#include "Util.h"
#include "Quaternion.h"
#include <math.h>


Util::Util()
{
}


Util::~Util()
{
}
 double Util::radians(double deg)
 {
	 return deg *PI / 180.0;
 }
 double Util::degrees(double rad)
 {
	 return rad* 180.0 / PI;
 }
 double Util::PI = 3.14159;
 
 Vector3 Util::EarthToBodyRates(double roll, double  pitch, double yaw, double rollRate, double  pitchRate, double yawRate)
 {
	 double phi = radians(roll);
	 double theta = radians(pitch);
	 double phiDot = radians(rollRate);
	 double thetaDot = radians(pitchRate);
	 double psiDot = radians(yawRate);

	 double p = phiDot - psiDot*sin(theta);
	 double q = cos(phi)*thetaDot + sin(phi)*psiDot*cos(theta);
	 double r = cos(phi)*psiDot*cos(theta) - sin(phi)*thetaDot;

	 return Vector3(degrees(p), degrees(q), degrees(r));
 }
 EarthFrameRate Util::BodyRatesToEarth(double roll, double  pitch, double yaw, double pDeg, double  qDeg, double rDeg)
 {
	 double p = radians(pDeg);
	 double q = radians(qDeg);
	 double r = radians(rDeg);

	 double phi = radians(roll);
	 double theta = radians(pitch);

	 double phiDot = p + tan(theta)*(q*sin(phi) + r*cos(phi));
	 double thetaDot = q*cos(phi) - r*sin(phi);
	 if (fabs(cos(theta)) < 1.0e-20)
		 theta += 1.0e-10;
	 double psiDot = (q*sin(phi) + r*cos(phi)) / cos(theta);
	//phiDot is for roll;
	 //ThetaDot is for pitch 
	 //psiDot is for yaw
	 return EarthFrameRate(degrees(thetaDot), degrees(phiDot), degrees(psiDot));

 }
 //Vector3 Util::RPY_to_XYZ(double roll, double  pitch, double yaw, double length)
 //{
	// Vector3 v = Vector3(0, 0, length);
	// Quaternion q1, q2;
	// //v = q1.new_rotate_euler(-radians(pitch), 0, -radians(roll)) * v;
	// q1 = q1.new_rotate_euler(-radians(pitch), 0, -radians(roll));
	// v = Quaternion::mul(q1, v);
	// //v = q2.new_rotate_euler(0, radians(yaw), 0) * v;
	// q2 = q2.new_rotate_euler(0, radians(yaw), 0);
	// v = Quaternion::mul(q2, v);
	// return v;

 //}